#pragma once

#include "../core/i_alloc.h"
#include "../core/i_rand.h"
#include "../core/i_struct.h"
#include "../numeric/i_nullspace.h"
#include "../optimize/i_lm_s.h"
#include "../da/i_ransac.h"
#include "i_rot.h"
#include "i_util.h"

namespace idl
{
  /*This routine normalizes the Homography matrix H such that the vector representing H is unitized - store results in Hn*/
  template<typename T>
  inline void i_homography_unitize(const T H[9], T Hn[9])
  {
    i_unitize9(H, Hn);
  }

  /*This routine normalizes the Homography matrix H such that the vector representing H is unitized*/
  template<typename T>
  inline void i_homography_unitize(T H[9])
  {
    i_unitize9(H);
  }

  /*This routine computes the new 3x3 Homography matrix Hp from H such that Hp is the corresponding Homography 
    of a resample image. The resample scale factor is sf
    i.e.,
    x' = Hx, then
    (sf*x') = Hp * (sf*x)*/
  template<typename T>
  inline void i_homography_resample(const T H[9], T Hp[9], T sf)
  {
    /*upsample H to the scale level defined by "upsample_factor"*/
    Hp[0] = H[0];
    Hp[1] = H[1];
    Hp[2] = H[2] * sf;
    Hp[3] = H[3];
    Hp[4] = H[4];
    Hp[5] = H[5] * sf;
    Hp[6] = i_div(H[6], sf);
    Hp[7] = i_div(H[7], sf);
    Hp[8] = H[8];
  }

  /*Same as above but H is modified inplace*/
  template<typename T>
  inline void i_homography_resample(T H[9], T sf)
  {
    H[2] = H[2] * sf;
    H[5] = H[5] * sf;
    H[6] = i_div(H[6], sf);
    H[7] = i_div(H[7], sf);
  }

  /*This routine runs the denormalization step for 2D homography estimation
  Note that both Tx and Txp are 3x3 upper triangular similarity transformation*/
  template<typename T> 
  inline void i_homography_denormalization(const T Ht[9], const T Tx[9], const T Txp[9], T H[9])
  {
    T Txpi[9], HT[9];
    i_invert_3x3_upper_triangular(Txp, Txpi);
    i_mult_AB_3x3_3x3_w_B_upper_triangular(Ht, Tx, HT);
    i_mult_AB_3x3_3x3_w_A_upper_triangular(Txpi, HT, H);
  }

  /*Constraints the 3x3 homography matrix X such that xp = H*x.
  These functions gives the constraints from the rows of [xp]_x*H*x*/
  template<typename T> inline void i_homography_constraint_r0(const T x[2], const T xp[2], T v[9])
  {
    v[0] = v[1] = v[2] = (T)0.0; 
    v[3] = -x[0];
    v[4] = -x[1];
    v[5] = -(T)1.0;
    v[6] = (xp[1] * x[0]);
    v[7] = (xp[1] * x[1]);
    v[8] =  xp[1];
  }

  template<typename T> inline void i_homography_constraint_r1(const T x[2], const T xp[2], T v[9])
  {
    T mxp0 = -xp[0];
    v[0] =  (x[0]);
    v[1] =  (x[1]);
    v[2] =  (T)1.0;
    v[3] = v[4] = v[5] = (T)0.0;
    v[6] = mxp0 * x[0];
    v[7] = mxp0 * x[1];
    v[8] = mxp0;
  }

  template<typename T> inline void i_homography_constraint_r2(const T x[2], const T xp[2], T v[9])
  {
    T mxp1 = -xp[1];
    v[0] = mxp1 * x[0];
    v[1] = mxp1 * x[1];
    v[2] = mxp1;
    v[3] =  xp[0] * x[0];
    v[4] =  xp[0] * x[1];
    v[5] =  xp[0];
    v[6] =  v[7] = v[8] = (T)0.0;
  }

  template<typename T>
  inline void i_homography_constraint(const T x[2], const T xp[2], T v1[9], T v2[9])
  {
    /*constraints from the two rows of [xp]_x*H*x that do not correspond to the largest coordinate of xp*/
    T x0 = i_abs(xp[0]);
    T x1 = i_abs(xp[1]);

    if (x0 >= x1)
    {
      if (x0 >= (T)1.0)
      {
        i_homography_constraint_r1(x, xp, v1);
        i_homography_constraint_r2(x, xp, v2);
      }
      else
      {
        i_homography_constraint_r0(x, xp, v1);
        i_homography_constraint_r1(x, xp, v2);
      }
    }
    else
    {
      if (x1 >= (T)1.0)
      {
        i_homography_constraint_r0(x, xp, v1);
        i_homography_constraint_r2(x, xp, v2);
      }
      else
      {
        i_homography_constraint_r0(x, xp, v1);
        i_homography_constraint_r1(x, xp, v2);
      }
    }
  }


  /*Compute the symmetric transfer error (distance in pixel) given two image points and the homography*/
  template<typename T> inline T i_symmetric_transfer_distance_homography(const T x1[2], const T x2[2], const T H[9])
  {
    T l[3], err1, err2, Hi[9];

    i_lu_invert_3x3(H, Hi);

    l[0] = H[0] * x1[0] + H[1] * x1[1] + H[2];
    l[1] = H[3] * x1[0] + H[4] * x1[1] + H[5];
    l[2] = H[6] * x1[0] + H[7] * x1[1] + H[8];
    i_homogeneous_unitize3(l);

    err1 = i_sqrt( i_squaresum_diff2(x2, l) );

    l[0] = Hi[0] * x2[0] + Hi[3] * x2[1] + Hi[6];
    l[1] = Hi[1] * x2[0] + Hi[4] * x2[1] + Hi[7];
    l[2] = Hi[2] * x2[0] + Hi[5] * x2[1] + Hi[8];
    i_homogeneous_unitize3(l);
    
    err2 = i_sqrt(i_squaresum_diff2(x1, l));

    return i_average(err1, err2);
  }

  /*This routine solves the four point homography matrix:
  Compute homography matrix H such that xp~=Hx given 4 point correspondences x<->xp.
  The points are given as triplets and quadruplets of homogeneous coordinates stored in the rows of x and xp,
  Note that The user is responsible for applying data normalization to the homogeneous coordinates
  to something good.
  If nullspace_method is :
  0, LU is used to find the nullspace, faster but less accurate
  1, SVD is used to find the nullspace, slower but more accurate
  else, always use SVD*/
  template<typename T>
  inline void i_four_point_homography(const T x[8], const T xp[8], T H[9], int null_space_method = 1)
  {
    T A[72]; /*8x9*/
    /*4 point constraints*/
    i_homography_constraint(x,     xp,     A,      A + 9);
    i_homography_constraint(x + 2, xp + 2, A + 18, A + 27);
    i_homography_constraint(x + 4, xp + 4, A + 36, A + 45);
    i_homography_constraint(x + 6, xp + 6, A + 54, A + 63);
    switch (null_space_method)
    {
    case 0:  i_nullspace_lu_8x9_destroy(A, H); break;
    case 1:  i_nullspace_svd_8x9(A, H);        break;
    default: i_nullspace_svd_8x9(A, H);        break;
    }
  }

  /*RANSAC solvers:*/

  /*This routine solves the four point homography matrix:
  Compute homography matrix H such that xp~=Hx given 4 point correspondences x<->xp.
  The points are given as twosome of inhomogeneous coordinates stored in the rows of x and xp,
  Note that the routine is responsible for applying data normalization to the homogeneous coordinates*/
  template<typename T>
  inline void i_four_point_homography_ransac_hypogenfunc(const T x[8], const T xp[8], T H[9])
  {
    T nx[8], nxp[8], Tx[9], Txp[9], Ht[9];
    //normalize input image coordinates to the normalized space;
    i_point_preconditioning4_inhomogeneous(x,  nx,  Tx);
    i_point_preconditioning4_inhomogeneous(xp, nxp, Txp);
    //solve the four-point homography problem
    i_four_point_homography(nx, nxp, Ht, 0);
    //denormalization: 
    i_homography_denormalization(Ht, Tx, Txp, H);
  }

  /*Compute the cost - sum of point to point distance in one view (only for inliers) for the Ransac routine
    candidate points are stored in data in inhomogeneous coordinate system*/
  template<typename T>
  inline void i_homography_ransac_oneview_costfunc(const T H[9], const T* x, const T* xp, int n, int &nr_liner, int *inliers, T &cost, T tol)
  {
    int i;
    T hx[3];
    T tolsqr = i_sqr(tol);
    T point_to_point_sqr_distance = (T)0.0;
    nr_liner = 0;
    cost = (T)0.0;
    const T* px = x;
    const T* pxp = xp;

    T h0 = H[0];
    T h1 = H[1];
    T h2 = H[2];
    T h3 = H[3];
    T h4 = H[4];
    T h5 = H[5];
    T h6 = H[6];
    T h7 = H[7];
    T h8 = H[8];

    for (i = 0; i < n; i++)
    {
      /*mult*/
      hx[0] = h0 * px[0] + h1 * px[1] + h2;
      hx[1] = h3 * px[0] + h4 * px[1] + h5;
      hx[2] = h6 * px[0] + h7 * px[1] + h8;
      i_homogeneous_unitize3(hx);
      
      point_to_point_sqr_distance = i_squaresum_diff2(pxp, hx);
      
      if (point_to_point_sqr_distance < tolsqr)
      {
        inliers[nr_liner++] = i;
        cost += point_to_point_sqr_distance;
      }

      px += 2;
      pxp += 2;
    }
  }


  /*Compute the cost - point to point distance in one view for the Preemptive Ransac routine
   candidate points are stored in data in inhomogeneous coordinate system*/
  template<typename T>
  inline void i_homography_preemptive_ransac_oneview_costfunc(const T H[9], const T x[2], const T xp[2], T &cost)
  {
    T hx[3];
    /*mult*/
    hx[0] = H[0] * x[0] + H[1] * x[1] + H[2];
    hx[1] = H[3] * x[0] + H[4] * x[1] + H[5];
    hx[2] = H[6] * x[0] + H[7] * x[1] + H[8];
    i_homogeneous_unitize3(hx);
    cost = i_squaresum_diff2(xp, hx);
  }


  /*Using Ransac to fit a model to data set which contains outliers
  The function needs 2n entries of scratch space in inliers*/
  template <typename T>
  bool i_est_homography_ransac(T* x,
                               T* xp,
                               int n, /*total # of samples*/
                               T model[9], /*the model, which contains k parameters*/
                               int &consensus_size, /*return the number of inliers in data*/
                               int *inliers, /*scratch space of length 2*n to store the indices of inliers*/
                               T error_tol,
                               int max_nr_trials,
                               double confidence = 0.99,
                               double inlierprob = 0.5,/*initial guess for the inliers probability (usually worst case)*/
                               int min_nr_inliers = 4,
                               bool random_shuffle_inputs = false)
  {
      int indices[4], offset;
      T samples_x[8];
      T samples_xp[8];
      T tmp_model[9];
      T cost = Constant<T>::MAX_VAL();
      T best_cost = Constant<T>::MAX_VAL();
      if (n < min_nr_inliers)
      { 
        return false;
      }
      
      int nr_trials = i_min(max_nr_trials, i_ransac_trials(4, confidence, inlierprob));

      int nr_inliers = 0;
      int rseed = I_DEFAULT_SEED;
      int sample_count = 0;

      consensus_size = 0; /*initialize the size of the consensus set to zero*/
      i_zero9(model); /*initialize the model with zeros*/

      if (random_shuffle_inputs)
      {
        i_randomized_shuffle(x, xp, n, 2, 2, rseed);
      }

      while (nr_trials > sample_count)
      {
        /*generate random indices*/
        i_random_sample(indices, 4, n, rseed);
        
        /*prepare data for model fitting - 4 pairs*/
        offset = (indices[0] << 1);
        i_copy2(x  + offset, samples_x);
        i_copy2(xp + offset, samples_xp);

        offset = (indices[1] << 1);
        i_copy2(x + offset,  samples_x + 2);
        i_copy2(xp + offset, samples_xp + 2);

        offset = (indices[2] << 1);
        i_copy2(x + offset,  samples_x + 4);
        i_copy2(xp + offset, samples_xp + 4);

        offset = (indices[3] << 1);
        i_copy2(x + offset,  samples_x + 6);
        i_copy2(xp + offset, samples_xp + 6);

        /*estimate model*/
        i_four_point_homography_ransac_hypogenfunc(samples_x, samples_xp, tmp_model);

        /*validate model*/
        i_homography_ransac_oneview_costfunc(tmp_model, x, xp, n, nr_inliers, inliers + n/*use the second half of scratch space - that's why the size of "inliers" is 2*n*/, cost, error_tol);
        if ((nr_inliers > consensus_size) || (nr_inliers == consensus_size && cost < best_cost)) /*found a better model*/
        {
          consensus_size = nr_inliers;
          best_cost = cost;
          i_copy9(tmp_model, model);
          i_copy(inliers + n, inliers, consensus_size);/*record inlier indices*/
        }
        sample_count++; 
      }
      return consensus_size >= min_nr_inliers;/*(i_div((double)consensus_size, n) >= inlierprob) -> too restricted*/;
    }

  template <typename T,
                 int m, /*# of total hypothesis*/
                 int b /*block size - suggested value 0.2*m, b must be less or equal to m*/>
  bool i_est_homography_preemptive_ransac(T* x,
                                          T* xp,
                                          int n, /*total # of samples*/
                                          T model[9], /*the model, which contains k parameters*/
                                          int &consensus_size, /*return the number of inliers in data*/
                                          int *inliers, /*scratch space of length n to store the indices of inliers*/
                                          T error_tol, /*warping error in pixel in the image space*/
                                          int min_nr_inliers = 4)
  {
    int i, j, i2, fi, fi_half, ioverb, indices[4], best_index, best_num, offset;
    int rseed = I_DEFAULT_SEED;
    T samples_x[8];
    T samples_xp[8];
    T models[9 * m];
    T *px, *pxp, cost;
    T tolsqr = i_sqr(error_tol);
    Pair<int, int> counts[m];
    consensus_size = 0; /*initialize the size of the consensus set to zero*/
    i_zero9(model); /*initialize the model with zeros*/
    
    if (b > m || n < min_nr_inliers)
    {
      return false;
    }

    /*random shuffle the inputs*/
    i_randomized_shuffle22(x, xp, n, rseed);
    
    /*hypothesis generation*/
    for (i = 0; i < m; ++i)
    {
      i_random_sample(indices, 4, n, rseed);

      /*prepare data for model fitting - 4 pairs*/
      offset = (indices[0] << 1);
      i_copy2(x + offset, samples_x);
      i_copy2(xp + offset, samples_xp);

      offset = (indices[1] << 1);
      i_copy2(x + offset, samples_x + 2);
      i_copy2(xp + offset, samples_xp + 2);

      offset = (indices[2] << 1);
      i_copy2(x + offset, samples_x + 4);
      i_copy2(xp + offset, samples_xp + 4);

      offset = (indices[3] << 1);
      i_copy2(x + offset, samples_x + 6);
      i_copy2(xp + offset, samples_xp + 6);

      /*generate the ith model hypothesis*/
      i_four_point_homography_ransac_hypogenfunc(samples_x, samples_xp, models + 9*i);

      /*initialize the count buffer*/
      counts[i].first = i; /*initialize to index id*/
      counts[i].second = 0; /*initialize to count 0*/
    }

    /*test hypothesis*/
    i = ioverb = 0;
    fi = m;

    while (i < n && fi > 1)
    {
      i2 = (i << 1);
      px = x + i2;
      pxp = xp + i2;

      for (j = 0; j < fi; ++j)
      {
        i_homography_preemptive_ransac_oneview_costfunc(models + (counts[j].first * 9), px, pxp, cost);
        if (cost < tolsqr)
        {
          counts[j].second++;
        }
      }

      ++i;

      if (ioverb < i / b)
      {
        ioverb++;
        fi_half = (fi >> 1); /*shift right -> divided by 2*/
        std::partial_sort(counts, counts + fi_half, counts + fi, i_larger_pair_second_element<int, int>);
        fi = fi_half;
      }
    }

    best_index = counts[0].first;
    best_num = counts[0].second;

    if (n == i && fi > 1)
    {
      for (j = 1; j < fi; ++j)
      {
        if (counts[j].second > best_num)
        {
          best_index = counts[j].first;
          best_num = counts[j].second;
        }
      }
    }

    i_copy9(models + (best_index*9), model); /*after the previous iteration, the first model is the best solution with max # of supports - partial_sort guarantees sorted first-half*/

    /*reassemble inliers*/
    px = x;
    pxp = xp;
    for (j = 0; j < n; ++j)
    {
      i_homography_preemptive_ransac_oneview_costfunc(model, px, pxp, cost);
      if (cost < tolsqr)
      {
        inliers[consensus_size++] = j;
      }
      px  += 2;
      pxp += 2;
    }

    bool succeeded = consensus_size >= min_nr_inliers;
    return succeeded;
  }

  /*Polish solver:*/

  /*This routine optimize the 3x3 homography matrix with a set of 2D<->2D correspondeces using the Levenberg-Marquardt algorithm.
  The input image points {xs, xps} should be in inhomogeneous coordinates.*/
  template<typename T>
  class HomographyOptimizer
  {
  public:
    typedef T data_type;
    static const int _m = 9;
    static const int _d = 2;
    HomographyOptimizer();
    ~HomographyOptimizer();
    void computeCost(const T *c, T *fvec) const;
    void computeJacobian(const T *c, T *J) const;
    void computeJointJacobianCost(const T *c, T *fvec, T *J) const;
    int getNumUnknowns() const { return _m; }
    int getNumberResiduals() const { return _d*_n; };
    bool optimize(const T H[9], const T *xs, const T *xps, int n);
    void getHomography(T H[9]) const;
    void setVerbose(bool verbose) { _verbose = verbose; }
    bool isFailed() const { return !_succeeded; }
  protected:
    T _c[_m]; /*9 parameters to model the 3x3 matrix H*/
    int _n;
    const T *_xs;
    const T *_xps;
    bool _succeeded;
    bool _verbose;
  };

  template<typename T>
  HomographyOptimizer<T>::HomographyOptimizer() : _n(0), _xs(NULL), _xps(NULL), _succeeded(false), _verbose(false)
  {
    i_zero(_c, _m);
  }

  template<typename T>
  HomographyOptimizer<T>::~HomographyOptimizer(){}

  template<typename T>
  void HomographyOptimizer<T>::getHomography(T H[9]) const
  {
    i_copy9(_c, H);
  }

  template<typename T>
  bool HomographyOptimizer<T>::optimize(const T H[9]/*initial homography matrix*/, const T* xs, const T* xps, int n/*# of 2D<->2D correspondeces*/)
  {
    _succeeded = false;
    if (n == 0 || xs == NULL || xps == NULL)
    {
      return false;
    }
    /*alloc error vector fvec*/
    T* fvec = i_alloc<T>(n*_d);
    if (fvec == NULL)
    {
      return false;
    }
    _n = n;
    _xs = xs;
    _xps = xps;
    i_copy9(H, _c);
    LevenbergMarquardtS solver;
    solver.setVerbose(_verbose);
    _succeeded = solver.optimize(*this, _c, fvec);
    i_free(fvec);
    return _succeeded;
  }

  /*use single view error for speed consideration*/
  template<typename T>
  void HomographyOptimizer<T>::computeCost(const T *c, T *fvec) const
  {
    const T& h0 = c[0];
    const T& h1 = c[1];
    const T& h2 = c[2];
    const T& h3 = c[3];
    const T& h4 = c[4];
    const T& h5 = c[5];
    const T& h6 = c[6];
    const T& h7 = c[7];
    const T& h8 = c[8];
    T hx0, hx1, hx2;

    const T *cptr_x = _xs;
    const T *cptr_xp = _xps;
    T *ptr_f = fvec;

    for (int i = 0; i < _n; ++i)
    {
      hx0 = h0*cptr_x[0] + h1*cptr_x[1] + h2;
      hx1 = h3*cptr_x[0] + h4*cptr_x[1] + h5;
      hx2 = h6*cptr_x[0] + h7*cptr_x[1] + h8;
      ptr_f[0] = i_div(hx0, hx2) - cptr_xp[0];
      ptr_f[1] = i_div(hx1, hx2) - cptr_xp[1];
      cptr_x += 2;
      cptr_xp += 2;
      ptr_f += _d;
    }
  }

  template<typename T>
  void HomographyOptimizer<T>::computeJacobian(const T *c, T *J) const
  {
    /*J is 2*_n x 9*/
    int i, j_offset = _m * _d;
    const T& h0 = c[0];
    const T& h1 = c[1];
    const T& h2 = c[2];
    const T& h3 = c[3];
    const T& h4 = c[4];
    const T& h5 = c[5];
    const T& h6 = c[6];
    const T& h7 = c[7];
    const T& h8 = c[8];
    T hx0, hx1, hx2, x, y, sf, sfx, sfy;

    const T *cptr_x = _xs;
    T* ptr_J = J;

    for (i = 0; i < _n; ++i)
    {
      x = cptr_x[0];
      y = cptr_x[1];

      hx0 = h0*x + h1*y + h2;
      hx1 = h3*x + h4*y + h5;
      hx2 = h6*x + h7*y + h8;
      sf = i_rec(hx2*hx2);

      sfx = sf * x;
      sfy = sf * y;

      /*dx/dc*/
      ptr_J[0] = hx2 * sfx; //(x * hx2)*sf;
      ptr_J[1] = hx2 * sfy; //(y * hx2)*sf;
      ptr_J[2] = (hx2)*sf;
      ptr_J[3] = (T)0.0;
      ptr_J[4] = (T)0.0;
      ptr_J[5] = (T)0.0;
      ptr_J[6] = -(hx0 * sfx); //(-hx0 * x)*sf;
      ptr_J[7] = -(hx0 * sfy); //(-hx0 * y)*sf;
      ptr_J[8] = -(hx0 * sf);

      /*dy/dc*/
      ptr_J[9] = (T)0.0;
      ptr_J[10] = (T)0.0;
      ptr_J[11] = (T)0.0;
      ptr_J[12] = ptr_J[0];
      ptr_J[13] = ptr_J[1];
      ptr_J[14] = ptr_J[2];
      ptr_J[15] = -(hx1 * sfx);//(-hx1 * x)*sf;
      ptr_J[16] = -(hx1 * sfy);//(-hx1 * y)*sf;
      ptr_J[17] = -(hx1* sf);

      cptr_x += 2;
      ptr_J += j_offset;
    }
  }

  template<typename T>
  void HomographyOptimizer<T>::computeJointJacobianCost(const T *c, T *fvec, T *J) const
  {
    /*J is 2*_n x 9*/
    int i, j_offset = _m * _d;

    const T& h0 = c[0];
    const T& h1 = c[1];
    const T& h2 = c[2];
    const T& h3 = c[3];
    const T& h4 = c[4];
    const T& h5 = c[5];
    const T& h6 = c[6];
    const T& h7 = c[7];
    const T& h8 = c[8];
    T hx0, hx1, hx2, x, y, sf, sfx, sfy;

    const T *cptr_x = _xs;
    const T *cptr_xp = _xps;
    T *ptr_f = fvec;
    T* ptr_J = J;

    for (i = 0; i < _n; ++i)
    {
      x = cptr_x[0];
      y = cptr_x[1];

      hx0 = h0*x + h1*y + h2;
      hx1 = h3*x + h4*y + h5;
      hx2 = h6*x + h7*y + h8;
      sf = i_rec(hx2*hx2);

      sfx = sf * x;
      sfy = sf * y;

      /*Cost:*/
      ptr_f[0] = i_div(hx0, hx2) - cptr_xp[0];
      ptr_f[1] = i_div(hx1, hx2) - cptr_xp[1];

      /*Jacobian:*/
      /*dx/dc*/
      ptr_J[0] = hx2 * sfx; //(x * hx2)*sf;
      ptr_J[1] = hx2 * sfy; //(y * hx2)*sf;
      ptr_J[2] = (hx2)*sf;
      ptr_J[3] = (T)0.0;
      ptr_J[4] = (T)0.0;
      ptr_J[5] = (T)0.0;
      ptr_J[6] = -(hx0 * sfx); //(-hx0 * x)*sf;
      ptr_J[7] = -(hx0 * sfy); //(-hx0 * y)*sf;
      ptr_J[8] = -(hx0 * sf);

      /*dy/dc*/
      ptr_J[9] = (T)0.0;
      ptr_J[10] = (T)0.0;
      ptr_J[11] = (T)0.0;
      ptr_J[12] = ptr_J[0];
      ptr_J[13] = ptr_J[1];
      ptr_J[14] = ptr_J[2];
      ptr_J[15] = -(hx1 * sfx);//(-hx1 * x)*sf;
      ptr_J[16] = -(hx1 * sfy);//(-hx1 * y)*sf;
      ptr_J[17] = -(hx1* sf);

      cptr_x += 2;
      cptr_xp += 2;
      ptr_f += _d;
      ptr_J += j_offset;
    }
  }

  /*This routine optimizes the pose ([R|t] from homography induced by a plane) with a set of 2D<->2D correspondeces using the Levenberg-Marquardt algorithm {xps} = H*{xs}.
  The routine assumes all 2D points {xs} lie in one plane with Z=0, and surface normal [0, 0, 1]
  The input image points {xps} should have been normalized (multiplied by K^-1) and in inhomogeneous coordinates. {xs} are in original image coordinates, no normalization (multiplied by K^-1) should be applied to {xs}.*/
  template<typename T>
  class CalibratedHomographyPoseOptimizer
  {
  public:
    typedef T data_type;
    static const int _m = 6;
    static const int _d = 2;
    CalibratedHomographyPoseOptimizer();
    ~CalibratedHomographyPoseOptimizer();
    void computeCost(const T *c, T *fvec) const;
    void computeJacobian(const T *c, T *J) const;
    void computeJointJacobianCost(const T *c, T *fvec, T *J) const;
    int getNumUnknowns() const { return _m; }
    int getNumberResiduals() const { return _d*_n; };
    bool optimize(const T R[9], const T t[3], const T *xs, const T *xps, int n);
    void getPose(T R[9], T t[3]) const;
    void setVerbose(bool verbose) { _verbose = verbose; }
    bool isFailed() const { return !_succeeded; }
  protected:
    T _c[_m]; /*6 parameters to model the pose R and t*/
    int _n;
    const T *_xs;
    const T *_xps;
    bool _succeeded;
    bool _verbose;
  };

  template<typename T>
  CalibratedHomographyPoseOptimizer<T>::CalibratedHomographyPoseOptimizer() : _n(0), _xs(NULL), _xps(NULL), _succeeded(false), _verbose(false)
  {
    i_zero(_c, _m);
  }

  template<typename T>
  CalibratedHomographyPoseOptimizer<T>::~CalibratedHomographyPoseOptimizer(){}

  template<typename T>
  void CalibratedHomographyPoseOptimizer<T>::getPose(T R[9], T t[3]) const
  {
    i_rot_rodrigues_3x3(_c, R);
    i_copy3(_c + 3, t);
  }

  template<typename T>
  bool CalibratedHomographyPoseOptimizer<T>::optimize(const T R[9], const T t[3], const T* xs, const T* xps, int n/*# of 2D<->2D correspondeces*/)
  {
    T theta;
    _succeeded = false;
    if (n == 0 || xs == NULL || xps == NULL)
    {
      return false;
    }

    /*alloc error vector fvec*/
    T* fvec = i_alloc<T>(n*_d);
    if (fvec == NULL)
    {
      return false;
    }

    _n = n;
    _xs = xs;
    _xps = xps;

    i_rot_invert_rodrigues_3x3(R, _c, theta);
    i_copy3(t, _c + 3);

    LevenbergMarquardtS solver;
    solver.setVerbose(_verbose);
    _succeeded = solver.optimize(*this, _c, fvec);
    i_free(fvec);
    return _succeeded;
  }

  /*use single view error for speed consideration*/
  template<typename T>
  void CalibratedHomographyPoseOptimizer<T>::computeCost(const T *c, T *fvec) const
  {
    T R[9];
    i_rot_rodrigues_3x3(c, R);

    T& h0 = R[0];
    T& h1 = R[1];
    const T& h2 = c[3];
    T& h3 = R[3];
    T& h4 = R[4];
    const T& h5 = c[4];
    T& h6 = R[6];
    T& h7 = R[7];
    const T& h8 = c[5];
    T hx0, hx1, hx2;

    const T *cptr_x = _xs;
    const T *cptr_xp = _xps;
    T *ptr_f = fvec;

    for (int i = 0; i < _n; ++i)
    {
      hx0 = h0*cptr_x[0] + h1*cptr_x[1] + h2;
      hx1 = h3*cptr_x[0] + h4*cptr_x[1] + h5;
      hx2 = h6*cptr_x[0] + h7*cptr_x[1] + h8;
      ptr_f[0] = i_div(hx0, hx2) - cptr_xp[0];
      ptr_f[1] = i_div(hx1, hx2) - cptr_xp[1];
      cptr_x += 2;
      cptr_xp += 2;
      ptr_f += _d;
    }
  }

  template<typename T>
  void CalibratedHomographyPoseOptimizer<T>::computeJacobian(const T *c, T *J) const
  {
    /*J is 2*_n x 6*/
    int i, j_offset = _m * _d;

    T R[9], D[9][3], h1_dc[3], h2_dc[3], h3_dc[3];
    i_rot_rodrigues_3x3(c, R, D);

    T& h0 = R[0];
    T& h1 = R[1];
    const T& h2 = c[3];
    T& h3 = R[3];
    T& h4 = R[4];
    const T& h5 = c[4];
    T& h6 = R[6];
    T& h7 = R[7];
    const T& h8 = c[5];

    T hx0, hx1, hx2, x, y, sf;

    const T *cptr_x  = _xs;
    const T *cptr_xp = _xps;
    T* ptr_J = J;

    for (i = 0; i < _n; ++i)
    {
      x = cptr_x[0];
      y = cptr_x[1];

      hx0 = h0*x + h1*y + h2;
      hx1 = h3*x + h4*y + h5;
      hx2 = h6*x + h7*y + h8;
      sf = i_rec(hx2 * hx2);

      hx0 *= sf;
      hx1 *= sf;
      hx2 *= sf;

      /*compute derivatives wrt v1, v2, v3, t1, t2, and t3 using the chain-rule*/
      h1_dc[0] = D[0][0] * x + D[1][0] * y; /*derivative of v1*/
      h1_dc[1] = D[0][1] * x + D[1][1] * y; /*derivative of v2*/
      h1_dc[2] = D[0][2] * x + D[1][2] * y; /*derivative of v3*/
      
      h2_dc[0] = D[3][0] * x + D[4][0] * y; /*derivative of v1*/
      h2_dc[1] = D[3][1] * x + D[4][1] * y; /*derivative of v2*/
      h2_dc[2] = D[3][2] * x + D[4][2] * y; /*derivative of v3*/
      
      h3_dc[0] = D[6][0] * x + D[7][0] * y; /*derivative of v1*/
      h3_dc[1] = D[6][1] * x + D[7][1] * y; /*derivative of v2*/
      h3_dc[2] = D[6][2] * x + D[7][2] * y; /*derivative of v3*/
      
      /*dx/dc*/
      ptr_J[0] = (h1_dc[0] * hx2 - hx0 * h3_dc[0]);
      ptr_J[1] = (h1_dc[1] * hx2 - hx0 * h3_dc[1]);
      ptr_J[2] = (h1_dc[2] * hx2 - hx0 * h3_dc[2]);
      ptr_J[3] =  hx2;
      ptr_J[4] = (T)0;
      ptr_J[5] = -hx0;

      /*dy/dc*/
      ptr_J[6 ] = (h2_dc[0] * hx2 - hx1 * h3_dc[0]);
      ptr_J[7 ] = (h2_dc[1] * hx2 - hx1 * h3_dc[1]);
      ptr_J[8 ] = (h2_dc[2] * hx2 - hx1 * h3_dc[2]);
      ptr_J[9 ] = (T)0;
      ptr_J[10] =  hx2;
      ptr_J[11] = -hx1;

      cptr_x += 2;
      cptr_xp += 2;
      ptr_J += j_offset;
    }
  }

  template<typename T>
  void CalibratedHomographyPoseOptimizer<T>::computeJointJacobianCost(const T *c, T *fvec, T *J) const
  {
    /*J is 2*_n x 6*/
    int i, j_offset = _m * _d;

    T R[9], D[9][3], h1_dc[3], h2_dc[3], h3_dc[3];
    i_rot_rodrigues_3x3(c, R, D);

    T& h0 = R[0];
    T& h1 = R[1];
    const T& h2 = c[3];
    T& h3 = R[3];
    T& h4 = R[4];
    const T& h5 = c[4];
    T& h6 = R[6];
    T& h7 = R[7];
    const T& h8 = c[5];

    T hx0, hx1, hx2, x, y, sf;

    const T *cptr_x  = _xs;
    const T *cptr_xp = _xps;
    T *ptr_f = fvec;
    T* ptr_J = J;

    for (i = 0; i < _n; ++i)
    {
      x = cptr_x[0];
      y = cptr_x[1];

      hx0 = h0*x + h1*y + h2;
      hx1 = h3*x + h4*y + h5;
      hx2 = h6*x + h7*y + h8;
      sf = i_rec(hx2*hx2);

      /*Cost:*/
      ptr_f[0] = i_div(hx0, hx2) - cptr_xp[0];
      ptr_f[1] = i_div(hx1, hx2) - cptr_xp[1];

      /*Jacobian:*/
      hx0 *= sf;
      hx1 *= sf;
      hx2 *= sf;

      /*compute derivatives wrt v1, v2, v3, t1, t2, and t3 using the chain-rule*/
      h1_dc[0] = D[0][0] * x + D[1][0] * y; /*derivative of v1*/
      h1_dc[1] = D[0][1] * x + D[1][1] * y; /*derivative of v2*/
      h1_dc[2] = D[0][2] * x + D[1][2] * y; /*derivative of v3*/

      h2_dc[0] = D[3][0] * x + D[4][0] * y; /*derivative of v1*/
      h2_dc[1] = D[3][1] * x + D[4][1] * y; /*derivative of v2*/
      h2_dc[2] = D[3][2] * x + D[4][2] * y; /*derivative of v3*/

      h3_dc[0] = D[6][0] * x + D[7][0] * y; /*derivative of v1*/
      h3_dc[1] = D[6][1] * x + D[7][1] * y; /*derivative of v2*/
      h3_dc[2] = D[6][2] * x + D[7][2] * y; /*derivative of v3*/

      /*dx/dc*/
      ptr_J[0] = (h1_dc[0] * hx2 - hx0 * h3_dc[0]);
      ptr_J[1] = (h1_dc[1] * hx2 - hx0 * h3_dc[1]);
      ptr_J[2] = (h1_dc[2] * hx2 - hx0 * h3_dc[2]);
      ptr_J[3] = hx2;
      ptr_J[4] = (T)0;
      ptr_J[5] = -hx0;

      /*dy/dc*/
      ptr_J[6] = (h2_dc[0] * hx2 - hx1 * h3_dc[0]);
      ptr_J[7] = (h2_dc[1] * hx2 - hx1 * h3_dc[1]);
      ptr_J[8] = (h2_dc[2] * hx2 - hx1 * h3_dc[2]);
      ptr_J[9] = (T)0;
      ptr_J[10] = hx2;
      ptr_J[11] = -hx1;

      cptr_x += 2;
      cptr_xp += 2;
      ptr_f += _d;
      ptr_J += j_offset;
    }
  }


  /*This routine optimizes the relative pose ([R|t] from homography induced by a plane pi) with a set of 2D<->2D correspondeces using the Levenberg-Marquardt algorithm.
  The input image points {xs and xps} should have been normalized (multiplied by K^-1) and in inhomogeneous coordinates.
  The first camera is assumed to be canonical, i.e., [I|0]*/
  template<typename T>
  class CalibratedHomographyRelativePoseOptimizer
  {
  public:
    typedef T data_type;
    static const int _m = 6;
    static const int _d = 2;
    CalibratedHomographyRelativePoseOptimizer();
    ~CalibratedHomographyRelativePoseOptimizer();
    void computeCost(const T *c, T *fvec) const;
    void computeJacobian(const T *c, T *J) const;
    void computeJointJacobianCost(const T *c, T *fvec, T *J) const;
    int getNumUnknowns() const { return _m; }
    int getNumberResiduals() const { return _d*_n; };
    bool optimize(const T R[9], const T t[3], const T plane[4], const T *xs, const T *xps, int n);/*the first camera  is canonical, i.e., [I|0]*/
    void getRelativePose(T R[9], T t[3]) const;
    void setVerbose(bool verbose) { _verbose = verbose; }
    bool isFailed() const { return !_succeeded; }
  protected:
    T _c[_m]; /*6 parameters to model the relative pose R and t between two cameras, the first camera is canonical, i.e., [I|0]*/
    T _pi[4];
    int _n;
    const T *_xs;
    const T *_xps;
    bool _succeeded;
    bool _verbose;
  };

  template<typename T>
  CalibratedHomographyRelativePoseOptimizer<T>::CalibratedHomographyRelativePoseOptimizer() : _n(0), _xs(NULL), _xps(NULL), _succeeded(false), _verbose(false)
  {
    i_zero(_c, _m);
  }

  template<typename T>
  CalibratedHomographyRelativePoseOptimizer<T>::~CalibratedHomographyRelativePoseOptimizer(){}

  template<typename T>
  void CalibratedHomographyRelativePoseOptimizer<T>::getRelativePose(T R[9], T t[3]) const
  {
    i_rot_rodrigues_3x3(_c, R);
    i_copy3(_c + 3, t);
  }

  /*Note that plane is in the coordinate space defined by the first canonical camera [I|0]*/
  template<typename T>
  bool CalibratedHomographyRelativePoseOptimizer<T>::optimize(const T R[9], const T t[3], const T plane[4], const T *xs, const T *xps, int n/*# of 2D<->2D correspondeces*/)
  {
    T theta, sf;
    _succeeded = false;
    if (n == 0 || xs == NULL || xps == NULL) 
    {
      return false;
    }

    /*alloc error vector fvec*/
    T* fvec = i_alloc<T>(n*_d);
    if (fvec == NULL)
    {
      return false;
    }

    /*normalize plane equation such that plane equation has the format [n, 1]*/
    sf = i_rec(i_l2_norm3(plane));
    i_scale4(plane, _pi, sf);

    if (i_abs(_pi[3]) < Constant<T>::MIN_ABS_SAFE_DIVIDEND())
    {
      return false; /*note that if plane[3] = 0, then the plane passes through the center of projection of the first camera at (0, 0, 0, 1), which is not a valid camera configuration and is not likely to happen in real case*/
    }

    i_homogeneous_unitize4(_pi);

    _n = n;
    _xs = xs;
    _xps = xps;

    i_rot_invert_rodrigues_3x3(R, _c, theta);
    i_copy3(t, _c + 3);

    LevenbergMarquardtS solver;
    solver.setVerbose(_verbose);
    _succeeded = solver.optimize(*this, _c, fvec);
    i_free(fvec);
    return _succeeded;
  }

  /*use single view error for speed consideration*/
  template<typename T>
  void CalibratedHomographyRelativePoseOptimizer<T>::computeCost(const T *c, T *fvec) const
  {
    T R[9], U[9];
    i_rot_rodrigues_3x3(c, R);
    i_mult_AB_3x1_1x3(c + 3, _pi, U);
    i_sub9(U, R);

    T& h0 = R[0];
    T& h1 = R[1];
    T& h2 = R[2];
    T& h3 = R[3];
    T& h4 = R[4];
    T& h5 = R[5];
    T& h6 = R[6];
    T& h7 = R[7];
    T& h8 = R[8];
    T hx0, hx1, hx2;
    const T *cptr_x = _xs;
    const T *cptr_xp = _xps;
    T *ptr_f = fvec;

    for (int i = 0; i < _n; ++i)
    {
      hx0 = h0*cptr_x[0] + h1*cptr_x[1] + h2;
      hx1 = h3*cptr_x[0] + h4*cptr_x[1] + h5;
      hx2 = h6*cptr_x[0] + h7*cptr_x[1] + h8;
      ptr_f[0] = i_div(hx0, hx2) - cptr_xp[0];
      ptr_f[1] = i_div(hx1, hx2) - cptr_xp[1];
      cptr_x += 2;
      cptr_xp += 2;
      ptr_f += _d;
    }
  }

  template<typename T>
  void CalibratedHomographyRelativePoseOptimizer<T>::computeJacobian(const T *c, T *J) const
  {
    /*J is 2*_n x 6*/
    int i, j_offset = _m * _d;

    T R[9], U[9], D[9][3], h1_dc[3], h2_dc[3], h3_dc[3], hx0, hx1, hx2, x, y, sf, v0xpv1ypv2;
    i_rot_rodrigues_3x3(c, R, D);
    i_mult_AB_3x1_1x3(c + 3, _pi, U);
    i_sub9(U, R);

    T v0 = _pi[0];
    T v1 = _pi[1];
    T v2 = _pi[2];

    T& h0 = R[0];
    T& h1 = R[1];
    T& h2 = R[2];
    T& h3 = R[3];
    T& h4 = R[4];
    T& h5 = R[5];
    T& h6 = R[6];
    T& h7 = R[7];
    T& h8 = R[8];
    
    const T *cptr_x = _xs;
    const T *cptr_xp = _xps;
    T* ptr_J = J;

    for (i = 0; i < _n; ++i)
    {
      x = cptr_x[0];
      y = cptr_x[1];

      hx0 = h0*x + h1*y + h2;
      hx1 = h3*x + h4*y + h5;
      hx2 = h6*x + h7*y + h8;
      sf = i_rec(hx2 * hx2);

      hx0 *= sf;
      hx1 *= sf;
      hx2 *= sf;

      v0xpv1ypv2 = -(v0*x + v1*y + v2);

      /*compute derivatives wrt v1, v2, v3, t1, t2, and t3 using the chain-rule*/
      h1_dc[0] = D[0][0] * x + D[1][0] * y + D[2][0]; 
      h1_dc[1] = D[0][1] * x + D[1][1] * y + D[2][1]; 
      h1_dc[2] = D[0][2] * x + D[1][2] * y + D[2][2];
      /*h1_dc[3] = v0xpv1ypv2;
      h1_dc[4] = 0; 
      h1_dc[5] = 0; */

      h2_dc[0] = D[3][0] * x + D[4][0] * y + D[5][0];
      h2_dc[1] = D[3][1] * x + D[4][1] * y + D[5][1];
      h2_dc[2] = D[3][2] * x + D[4][2] * y + D[5][2];
      /*h2_dc[3] = 0;
      h2_dc[4] = v0xpv1ypv2;
      h2_dc[5] = 0;*/

      h3_dc[0] = D[6][0] * x + D[7][0] * y + D[8][0];
      h3_dc[1] = D[6][1] * x + D[7][1] * y + D[8][1];
      h3_dc[2] = D[6][2] * x + D[7][2] * y + D[8][2];
      /*h3_dc[3] = 0;
      h3_dc[4] = 0;
      h3_dc[5] = v0xpv1ypv2;*/

      /*dx/dc*/
      ptr_J[0] = (h1_dc[0] * hx2 - hx0 * h3_dc[0]);
      ptr_J[1] = (h1_dc[1] * hx2 - hx0 * h3_dc[1]);
      ptr_J[2] = (h1_dc[2] * hx2 - hx0 * h3_dc[2]);
      ptr_J[3] = v0xpv1ypv2 * hx2;
      ptr_J[4] = 0;
      ptr_J[5] =  -(hx0 * v0xpv1ypv2);

      /*dy/dc*/
      ptr_J[6]  = (h2_dc[0] * hx2 - hx1 * h3_dc[0]);
      ptr_J[7]  = (h2_dc[1] * hx2 - hx1 * h3_dc[1]);
      ptr_J[8]  = (h2_dc[2] * hx2 - hx1 * h3_dc[2]);
      ptr_J[9]  = 0;
      ptr_J[10] = ptr_J[3]; // v0xpv1ypv2 * hx2;
      ptr_J[11] = -(hx1 * v0xpv1ypv2);

      cptr_x += 2;
      cptr_xp += 2;
      ptr_J += j_offset;
    }
  }

  template<typename T>
  void CalibratedHomographyRelativePoseOptimizer<T>::computeJointJacobianCost(const T *c, T *fvec, T *J) const
  {
    /*J is 2*_n x 6*/
    int i, j_offset = _m * _d;

    T R[9], U[9], D[9][3], h1_dc[3], h2_dc[3], h3_dc[3], hx0, hx1, hx2, x, y, sf, v0xpv1ypv2;
    i_rot_rodrigues_3x3(c, R, D);
    i_mult_AB_3x1_1x3(c + 3, _pi, U);
    i_sub9(U, R);

    T v0 = _pi[0];
    T v1 = _pi[1];
    T v2 = _pi[2];

    T& h0 = R[0];
    T& h1 = R[1];
    T& h2 = R[2];
    T& h3 = R[3];
    T& h4 = R[4];
    T& h5 = R[5];
    T& h6 = R[6];
    T& h7 = R[7];
    T& h8 = R[8];

    const T *cptr_x = _xs;
    const T *cptr_xp = _xps;
    T *ptr_f = fvec;
    T* ptr_J = J;

    for (i = 0; i < _n; ++i)
    {
      x = cptr_x[0];
      y = cptr_x[1];

      hx0 = h0*x + h1*y + h2;
      hx1 = h3*x + h4*y + h5;
      hx2 = h6*x + h7*y + h8;
      sf = i_rec(hx2 * hx2);

      /*Cost:*/
      ptr_f[0] = i_div(hx0, hx2) - cptr_xp[0];
      ptr_f[1] = i_div(hx1, hx2) - cptr_xp[1];

      /*Jacobian:*/
      hx0 *= sf;
      hx1 *= sf;
      hx2 *= sf;

      v0xpv1ypv2 = -(v0*x + v1*y + v2);

      /*compute derivatives wrt v1, v2, v3, t1, t2, and t3 using the chain-rule*/
      h1_dc[0] = D[0][0] * x + D[1][0] * y + D[2][0];
      h1_dc[1] = D[0][1] * x + D[1][1] * y + D[2][1];
      h1_dc[2] = D[0][2] * x + D[1][2] * y + D[2][2];
      /*h1_dc[3] = v0xpv1ypv2;
      h1_dc[4] = 0;
      h1_dc[5] = 0; */

      h2_dc[0] = D[3][0] * x + D[4][0] * y + D[5][0];
      h2_dc[1] = D[3][1] * x + D[4][1] * y + D[5][1];
      h2_dc[2] = D[3][2] * x + D[4][2] * y + D[5][2];
      /*h2_dc[3] = 0;
      h2_dc[4] = v0xpv1ypv2;
      h2_dc[5] = 0;*/

      h3_dc[0] = D[6][0] * x + D[7][0] * y + D[8][0];
      h3_dc[1] = D[6][1] * x + D[7][1] * y + D[8][1];
      h3_dc[2] = D[6][2] * x + D[7][2] * y + D[8][2];
      /*h3_dc[3] = 0;
      h3_dc[4] = 0;
      h3_dc[5] = v0xpv1ypv2;*/

      /*dx/dc*/
      ptr_J[0] = (h1_dc[0] * hx2 - hx0 * h3_dc[0]);
      ptr_J[1] = (h1_dc[1] * hx2 - hx0 * h3_dc[1]);
      ptr_J[2] = (h1_dc[2] * hx2 - hx0 * h3_dc[2]);
      ptr_J[3] = v0xpv1ypv2 * hx2;
      ptr_J[4] = (T)0;
      ptr_J[5] = -(hx0 * v0xpv1ypv2);

      /*dy/dc*/
      ptr_J[6] = (h2_dc[0] * hx2 - hx1 * h3_dc[0]);
      ptr_J[7] = (h2_dc[1] * hx2 - hx1 * h3_dc[1]);
      ptr_J[8] = (h2_dc[2] * hx2 - hx1 * h3_dc[2]);
      ptr_J[9] = (T)0;
      ptr_J[10] = ptr_J[3]; // v0xpv1ypv2 * hx2;
      ptr_J[11] = -(hx1 * v0xpv1ypv2);

      cptr_x += 2;
      cptr_xp += 2;
      ptr_f += _d;
      ptr_J += j_offset;
    }
  }
} /* namespace idl */